#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include "imu_factor.h"
#include "wheel_factor.h"

#include "pose_observe.h"
#include "state.h"

using namespace std;

class Optimizer {
public:
	Optimizer(std::shared_ptr<ImuPreIntegration> imu_preintegration, std::shared_ptr<WheelPreIntegration> wheel_preintegration);
	void OptimizeLM(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose);
private:
	double Chi2(const Eigen::Matrix<double, 5, 1>& residual, const Eigen::Matrix<double, 5, 5>& information_matrix);
	double Chi2(const Eigen::Matrix<double, 2, 1>& residual, const Eigen::Matrix<double, 2, 2>& information_matrix);
	double Chi2(const Eigen::Matrix<double, 3, 1>& residual, const Eigen::Matrix<double, 3, 3>& information_matrix);

	void update_state(State& last_state, State& state, const Eigen::Matrix<double, 13, 1>& dx);
	
	std::shared_ptr<ImuFactor> imu_factor_;
	std::shared_ptr<WheelFactor> wheel_factor_;
};

#endif
